﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using System.Net.Sockets;
using cszmcaux;


namespace HTCP_Scan_Frame
{
    /// <summary>
    /// 远程控制
    /// 使用UDP连接
    /// </summary>
    public  class RemoteControl
    {
        public int CheckZHomeTime = 20;
        public int CheckZHomeTimeS = 0;
        public bool CheckZHome = false;
        public bool b_Connect_Remo = false;
        public bool b_Connect_Remo2 = false;
        public float XAxis_Offset = -500;
        public float YAxis_Offset = -500;
        public float ZAxis_Offset = 10;
        bool[] HomeDone = new bool[3] { false, false, false };
        public int HomeStepX = 0;
        public int HomeStepY = 0;
        public int HomeStepZ = 0;
        private Socket UDPServer;
        private Socket UDPServer2;
        public  bool IsOpened = false;
        private Thread ReceiveThread;
        private Thread ReceiveThread2;
        private Thread AxishomeThread;
        private System.Net.EndPoint RemoteDevice = new System.Net.IPEndPoint(System.Net.IPAddress.Parse("10.0.0.111"), 60310);
        private System.Net.EndPoint RemoteDevice2 = new System.Net.IPEndPoint(System.Net.IPAddress.Parse("10.0.0.111"), 60310);
        float[] Ratio = new float[4] {1, 50, 50, 1 };

        /// <summary>
        /// 开启远程控制
        /// 启动UDPserver
        /// 开启接收线程
        /// 开启后IsOpen设为true
        /// </summary>
        /// <param name="IP">本机IP</param>
        /// <param name="Port">本机Port</param>
        /// <returns></returns>
        /// 

        public RemoteControl(string IP, string IP2)
        {
            RemoteDevice = new System.Net.IPEndPoint(System.Net.IPAddress.Parse(IP), 60310);
            RemoteDevice2 = new System.Net.IPEndPoint(System.Net.IPAddress.Parse(IP2), 60310);
        }
        public void StartAxishomeThread()
        {
            AxishomeThread = new Thread(AxishomeMove);
            AxishomeThread.Start();
        }

        public void StopAxishomeThread()
        {
            AxishomeThread.Abort();
            AxishomeThread.Join();
        }


        public void Start(string IP, int Port, string IP2, int Port2)
        {
            try
            {
                if (IsOpened) return;
                //开启udp服务
                UDPServer = new Socket(AddressFamily.InterNetwork, SocketType.Dgram, ProtocolType.Udp);
                UDPServer.Bind(new System.Net.IPEndPoint(System.Net.IPAddress.Parse(IP), Port));
                UDPServer2 = new Socket(AddressFamily.InterNetwork, SocketType.Dgram, ProtocolType.Udp);
                UDPServer2.Bind(new System.Net.IPEndPoint(System.Net.IPAddress.Parse(IP2), Port2));
                //开启接收线程
                ReceiveThread = new Thread(Receive);
                ReceiveThread.Start();
                ReceiveThread2 = new Thread(Receive2);
                ReceiveThread2.Start();
                IsOpened = true;
                //b_Connect_Remo = true;
                //b_Connect_Remo2 = true;
            }
            catch
            {

            }
        }
        public void Close()
        {

            if (!IsOpened) return;
            try
            {
                ReceiveThread.Abort();
                ReceiveThread.Join();
                UDPServer.Disconnect(true);
                UDPServer.Close();
                ReceiveThread2.Abort();
                ReceiveThread2.Join();
                UDPServer2.Disconnect(true);
                UDPServer2.Close();
                IsOpened = false;
                b_Connect_Remo2 = false;
                b_Connect_Remo = false;
            }
            catch
            {

            }
        }


        public void AxisMoveSingle(int AxisNum, float AxisSpeed, float AxisPos)
        {
            int ret = 0;
            if (AxisNum == 0 || AxisNum == 1)
            {
                ret = zmcaux.ZAux_Direct_SetUnits(Form1.g_handle, AxisNum, 10000);

                ret = zmcaux.ZAux_Direct_SetSpeed(Form1.g_handle, AxisNum, AxisSpeed);

                ret = zmcaux.ZAux_Direct_SetAccel(Form1.g_handle, AxisNum, 5 * AxisSpeed);

                ret = zmcaux.ZAux_Direct_Single_Move(Form1.g_handle, AxisNum, AxisPos);
            }
            else if (AxisNum == 2)
            {
                ret = zmcaux.ZAux_Direct_SetUnits(Form1.g_handle, AxisNum, 1);

                ret = zmcaux.ZAux_Direct_SetSpeed(Form1.g_handle, AxisNum, 10000 * AxisSpeed);

                ret = zmcaux.ZAux_Direct_SetAccel(Form1.g_handle, AxisNum, 50000 * AxisSpeed);

                ret = zmcaux.ZAux_Direct_Single_Move(Form1.g_handle, AxisNum, 10000 * AxisPos);
            }
            else
            {

            }
           
            
        }

         public void AxisX_HoneMove()
        {
            if (Form1.g_handle == (IntPtr)0)
            {
                Form1.g_Alarm = "未链接到控制器!";
                //return;
            }
            int ret = 0;
            ret = zmcaux.ZAux_Direct_SetUnits(Form1.g_handle, 0, 10000);
            ret = zmcaux.ZAux_Direct_SetAccel(Form1.g_handle, 0, 300);
            ret = zmcaux.ZAux_Direct_SetSpeed(Form1.g_handle, 0, 50);
            ret = zmcaux.ZAux_Direct_SetCreep(Form1.g_handle, 0, 30);
            ret = zmcaux.ZAux_BusCmd_SetDatumOffpos(Form1.g_handle, 0, XAxis_Offset);
            ret = zmcaux.ZAux_BusCmd_Datum(Form1.g_handle, 0, 2);
        }

        public void AxisY_HoneMove()
        {
            if (Form1.g_handle == (IntPtr)0)
            {
                Form1.g_Alarm = "未链接到控制器!";
                //return;
            }
            int ret = 0;
            ret = zmcaux.ZAux_Direct_SetUnits(Form1.g_handle, 1, 10000);
            ret = zmcaux.ZAux_Direct_SetAccel(Form1.g_handle, 1, 300);
            ret = zmcaux.ZAux_Direct_SetSpeed(Form1.g_handle, 1, 50);
            ret = zmcaux.ZAux_Direct_SetCreep(Form1.g_handle, 1, 30);
            ret = zmcaux.ZAux_BusCmd_SetDatumOffpos(Form1.g_handle, 1,0);
            ret = zmcaux.ZAux_BusCmd_Datum(Form1.g_handle, 1, 18);
        }
        public void AxisZ_HoneMove()
        {
            if (Form1.g_handle == (IntPtr)0)
            {
                Form1.g_Alarm ="未链接到控制器!";
                //return;
            }
            int ret = 0;
            ret = zmcaux.ZAux_Direct_SetUnits(Form1.g_handle, 2, 1);
            ret = zmcaux.ZAux_Direct_SetAccel(Form1.g_handle, 2, 1000000);
            ret = zmcaux.ZAux_Direct_SetSpeed(Form1.g_handle, 2, 200000);
            ret = zmcaux.ZAux_Direct_SetCreep(Form1.g_handle, 2, 50000);
            ret = zmcaux.ZAux_BusCmd_SetDatumOffpos(Form1.g_handle, 2, 0);
            ret = zmcaux.ZAux_BusCmd_Datum(Form1.g_handle, 2, 19);
        }

        public void AxisXHomeStep()
        {
            int ret = 0;
            int[] i_AxisPara = new int[3] { 1,1,1 };
            switch (HomeStepX)
            {
                case 10://轴回零开始
                    zmcaux.ZAux_Direct_SetUserVar(Form1.g_handle, "gAxisX_HomeDone", 0);          //发送幅值
                    AxisX_HoneMove();
                    HomeStepX = 20;
                    break;
                case 20://  全部回零中
                    ret = zmcaux.ZAux_Direct_GetAxisStatus(Form1.g_handle, 0, ref i_AxisPara[0]);
                    if (i_AxisPara[0] == 0)
                    {
                        ret = zmcaux.ZAux_Direct_SetUnits(Form1.g_handle, 0, 10000);
                        ret = zmcaux.ZAux_Direct_SetAccel(Form1.g_handle, 0, 300);
                        ret = zmcaux.ZAux_Direct_SetSpeed(Form1.g_handle, 0, 50);
                        ret = zmcaux.ZAux_Direct_SetCreep(Form1.g_handle, 0, 30);
                        ret = zmcaux.ZAux_BusCmd_SetDatumOffpos(Form1.g_handle, 0, 0);
                        ret = zmcaux.ZAux_BusCmd_Datum(Form1.g_handle,0, 35);
                        
                        HomeStepX = 30;
                        //AxisMoveAbs(0, 60, XAxis_Offset);
                    }
                    break;
                case 30://找零完成，偏移运行
                    ret = zmcaux.ZAux_Direct_GetAxisStatus(Form1.g_handle, 0, ref i_AxisPara[0]);
                    if (i_AxisPara[0] == 0)			//第6位是否被置
                    {
                        HomeStepX = 0;
                        Form1.g_HomeX = false;
                        //ret = zmcaux.ZAux_BusCmd_Datum(Form1.g_handle, 0, 35);
                        zmcaux.ZAux_Direct_SetUserVar(Form1.g_handle, "gAxisX_HomeDone", 1);          //发送幅值
                    }
                    break;
                default:
                    break;
            }
        }

        public void AxisYHomeStep()
        {
            int ret = 0;
            int[] i_AxisPara = new int[3] { 1, 1, 1 };
            switch (HomeStepY)
            {
                case 10://轴回零开始
                    zmcaux.ZAux_Direct_SetUserVar(Form1.g_handle, "gAxisY_HomeDone", 0);          //发送幅值
                    AxisY_HoneMove();
                    HomeStepY = 12;
                    break;
                case 12://轴回零开始
                    ret = zmcaux.ZAux_Direct_GetAxisStatus(Form1.g_handle, 1, ref i_AxisPara[1]);
                    if (i_AxisPara[1] == 0)
                    {
                        ret = zmcaux.ZAux_BusCmd_Datum(Form1.g_handle, 1, 35);
                        ret = zmcaux.ZAux_Direct_SetUnits(Form1.g_handle, 1, 10000);
                        ret = zmcaux.ZAux_Direct_SetAccel(Form1.g_handle, 1, 300);
                        ret = zmcaux.ZAux_Direct_SetDecel(Form1.g_handle, 1, 300);
                        ret = zmcaux.ZAux_Direct_SetSpeed(Form1.g_handle, 1, 50);
                        ret = zmcaux.ZAux_Direct_Single_MoveAbs(Form1.g_handle, 1, YAxis_Offset);
                        HomeStepY = 20;
                    }

                    
                    break;

                case 20://  全部回零中
                    ret = zmcaux.ZAux_Direct_GetAxisStatus(Form1.g_handle, 1, ref i_AxisPara[1]);
                    if (i_AxisPara[1] == 0)
                    {
                        ret = zmcaux.ZAux_BusCmd_Datum(Form1.g_handle, 1, 35);
                        HomeStepY = 0;
                        Form1.g_HomeY = false;
                        //ret = zmcaux.ZAux_BusCmd_Datum(Form1.g_handle, 1, 35);
                        zmcaux.ZAux_Direct_SetUserVar(Form1.g_handle, "gAxisY_HomeDone", 1);          //发送幅值
                        //HomeStepY = 30;
                        //AxisMoveAbs(1, 60, YAxis_Offset);
                    }
                    break;
                case 30://找零完成，偏移运行
                    ret = zmcaux.ZAux_Direct_GetAxisStatus(Form1.g_handle, 1, ref i_AxisPara[1]);
                    if (i_AxisPara[1] == 0)			//第6位是否被置
                    {

                    }
                    break;
                default:
                    break;
            }
        }

        public void AxisZHomeStep()
        {
            int ret = 0;
            int[] i_AxisPara = new int[3] { 1, 1, 1 };
            switch (HomeStepZ)
            {
                case 10://轴回零开始
                    zmcaux.ZAux_Direct_SetUserVar(Form1.g_handle, "gAxisZ_HomeDone", 0);          //发送幅值
                    AxisZ_HoneMove();
                    CheckZHome = false;
                    CheckZHomeTime = 0;
                    HomeStepZ = 20;
                    CheckZHomeTimeS = 0;
                    break;
                case 11://轴回零开始
                    zmcaux.ZAux_Direct_SetUserVar(Form1.g_handle, "gAxisZ_HomeDone", 0);          //发送幅值
                    AxisZ_HoneMove();
                    CheckZHome = false;
                    CheckZHomeTime = 0;
                    HomeStepZ = 20;
                    break;
                case 12:
                    zmcaux.ZAux_Direct_SetUserVar(Form1.g_handle, "gAxisZ_HomeDone", 0);          //发送幅值
                    HomeStepZ = 13;
                    AxisMoveSingle(2, 20, -20);
                    break;
                case 13:
                    zmcaux.ZAux_Direct_SetUserVar(Form1.g_handle, "gAxisZ_HomeDone", 0);          //发送幅值
                    ret = zmcaux.ZAux_Direct_GetAxisStatus(Form1.g_handle, 2, ref i_AxisPara[2]);
                    if (i_AxisPara[2] == 0)			//第6位是否被置
                    {
                        HomeStepZ = 10;
                    }
                    break;
                case 20://  全部回零中
                    zmcaux.ZAux_Direct_SetUserVar(Form1.g_handle, "gAxisZ_HomeDone", 0);          //发送幅值
                    ret = zmcaux.ZAux_Direct_GetAxisStatus(Form1.g_handle, 2, ref i_AxisPara[2]);
                    if (((i_AxisPara[2] & 64) == 64) && CheckZHome == false)
                    {
                        CheckZHomeTime++;
                        if (CheckZHomeTime > 5)
                        {
                            HomeStepZ = 21;
                            CheckZHome = true;
                            CheckZHomeTime = 0;
                        }
                        
                    }
                    else
                    {
                        CheckZHomeTimeS ++ ;
                        if (CheckZHomeTimeS > 3)
                        {
                            CheckZHomeTimeS = 0;
                            HomeStepZ = 12;
                        }
                        else
                        {
                            HomeStepZ = 11;
                        }
                        
                    }
                    break;
                case 21:
                    zmcaux.ZAux_Direct_SetUserVar(Form1.g_handle, "gAxisZ_HomeDone", 0);          //发送幅值
                    ret = zmcaux.ZAux_Direct_GetAxisStatus(Form1.g_handle, 2, ref i_AxisPara[2]);
                    if (i_AxisPara[2] == 0  )			//第6位是否被置
                    {
                        HomeStepZ = 30;
                        AxisMoveSingle(2, 20, ZAxis_Offset);
                    }
                    break;
                case 30://找零完成，偏移运行
                    ret = zmcaux.ZAux_Direct_GetAxisStatus(Form1.g_handle, 2, ref i_AxisPara[2]);
                    if (i_AxisPara[2] == 0)			//第6位是否被置
                    {
                        HomeStepZ = 0;
                        Form1.g_HomeZ = false;
                        ret = zmcaux.ZAux_BusCmd_Datum(Form1.g_handle, 2, 35);
                        zmcaux.ZAux_Direct_SetUserVar(Form1.g_handle, "gAxisZ_HomeDone", 1);          //发送幅值
                    }
                    break;
                default:
                    break;
            }
        }


       



        public void AxishomeMove()
        {
            while(true)
            {
                bool bConnect = false;
                float Bus_Connect = 0;
                if (Form1.g_handle == (IntPtr)0)
                {
                    Form1.g_Alarm = "未链接到控制器!";
                    //return;
                }
                else
                {
                    int ret = zmcaux.ZAux_Direct_GetUserVar(Form1.g_handle, "gBusConnect", ref Bus_Connect);						//读取BAS文件中的变量判断是否有加载BAS文件
                    if (ret == 0 && Bus_Connect == 1)
                    {
                        bConnect = true;

                    }
                    if (bConnect)
                    {
                        if (Form1.g_HomeX)
                        {
                            AxisXHomeStep();
                        }
                        if (Form1.g_HomeY)
                        {
                            AxisYHomeStep();
                        }
                        if (Form1.g_HomeZ)
                        {
                            AxisZHomeStep();
                        }
                    }
                }
                
                
                
                
                Thread.Sleep(50);
            }
        }


        public void Receive()
        {
            //监听来自远程设备的指令
            //解析指令
            byte[] Msg = new byte[32];
            while (true)
            {
                if (IsOpened)
                {
                    try
                    {
                        Array.Resize(ref Msg, 32);
                        Array.Clear(Msg, 0, 32);
                        int Length = UDPServer.ReceiveFrom(Msg, ref RemoteDevice);
                        Array.Resize(ref Msg, Length);
                        AnalyzeCMD(1, Msg);
                    }
                    catch
                    {
                    }
                }
            }

        }
        public void Receive2()
        {
            //监听来自远程设备的指令
            //解析指令
            byte[] Msg = new byte[32];
            while (true)
            {
                if (IsOpened)
                {
                    try
                    {
                        Array.Resize(ref Msg, 32);
                        Array.Clear(Msg, 0, 32);
                        int Length = UDPServer2.ReceiveFrom(Msg, ref RemoteDevice2);
                        Array.Resize(ref Msg, Length);
                        AnalyzeCMD(2, Msg);
                    }
                    catch
                    {
                    }
                }
            }

        }

        //数据Data的格式，长度20，Data[0]=0xeb,Data[19]=0xff,Data[18]=为校验和
        //Data[1],Data[2],Data[3] 表示轴的状态，bit0 是否在线(1在线 0不在线) bit1是否运动（1运动，0停止） bit2:bit3 是否寻零（1正在寻零，2寻零完成） bit4:bit5 是否正弦
        //Data[4-7] 表示横移位置值*100
        //Data[8-9] 表示横移速度值*10
        //Data[10-13] 表示纵移位置值*100
        //Data[14-15] 表示纵移速度值*10
        //Data[16-17] 表示滚转位置值*10


        public void SendData(byte[] Data)
        {

            //发送数据到远程设备
            if (!IsOpened) return;
            if (RemoteDevice != null)
            {
                try
                {
                    if (b_Connect_Remo)
                    {
                        UDPServer.SendTo(Data, RemoteDevice);
                    }
                    if (b_Connect_Remo2)
                    {
                        UDPServer2.SendTo(Data, RemoteDevice2);
                    }
                }
                catch
                {

                }
            }
        }




        //正弦云顶
        public void SinMove (float AxisNun, float Axis_Amplitudem, float Axis_Cycles_Time, float Axis_Cycles_Num)
        {
            float AxisX_HomeDone = 0;
            float AxisY_HomeDone = 0;
            if (Form1.g_handle == (IntPtr)0)
            {
                Form1.g_Alarm = "未链接到控制器!";
                //return;
            }

            if (AxisNun == 1)//横移
            {
                if (Form1.g_XSrevoOn == "未使能")
                {
                    Form1.g_Alarm = "横移轴未使能";
                    //return;
                }
                int ret = zmcaux.ZAux_Direct_GetUserVar(Form1.g_handle, "gAxisX_HomeDone", ref AxisX_HomeDone);
                if (AxisX_HomeDone != 1)
                {
                    Form1.g_Alarm = "横移轴未回零";
                    //return;
                }
            }

            if (AxisNun == 2)   //纵移轴
            {
                if (Form1.g_YSrevoOn == "未使能")
                {
                    Form1.g_Alarm = "纵移轴未使能";
                    //return;
                }
                int ret = zmcaux.ZAux_Direct_GetUserVar(Form1.g_handle, "gAxisY_HomeDone", ref AxisY_HomeDone);
                if (AxisY_HomeDone != 1)
                {
                    Form1.g_Alarm = "纵移轴未回零";
                    //return;
                }
            }
            
            try
            {
                zmcaux.ZAux_Direct_SetUserVar(Form1.g_handle, "gAxis_Amplitudem", Axis_Amplitudem);          //发送幅值
                zmcaux.ZAux_Direct_SetUserVar(Form1.g_handle, "gAxis_Cycles_Time", Axis_Cycles_Time);          //发送频率
                zmcaux.ZAux_Direct_SetUserVar(Form1.g_handle, "gAxis_Cycles_Num", Axis_Cycles_Num);          //发送运行周期数
                float cmd = 1;                                                                 //进入正弦运动

                if (AxisNun == 1)
                {
                    zmcaux.ZAux_Direct_GetUserVar(Form1.g_handle, "gStep", ref cmd);
                    if (cmd == 3 || cmd == 4 || cmd == 20)
                    {
                         Form1.g_Alarm = "纵移正在正弦运动";
                    }
                    else
                    {
                        cmd = 1;
                        zmcaux.ZAux_Direct_SetUserVar(Form1.g_handle, "gStep", cmd);          //发送运行周期数
                    }
                    
                }
                else if (AxisNun == 2)
                {
                    zmcaux.ZAux_Direct_GetUserVar(Form1.g_handle, "gStep", ref cmd);
                    if (cmd == 1 || cmd == 2 || cmd == 10)
                    {
                        Form1.g_Alarm = "横移正在正弦运动";
                    }
                    else
                    {
                        cmd = 3;
                        zmcaux.ZAux_Direct_SetUserVar(Form1.g_handle, "gStep", cmd);          //发送运行周期数
                    }
                    
                }

               
            }
            catch
            {
                Form1.g_Alarm = "参数输入错误错误";
            }
        }


        public void RemoHome(int ChoicesAxis)
        {
            if(ChoicesAxis ==0)
            {
                if (Form1.g_HomeX == false)
                {
                    
                    Form1.g_HomeX = true;
                    HomeStepX = 10;
                }
                if(Form1.g_HomeY == false)
                {
                    Form1.g_HomeY = true;
                    HomeStepY = 10;
                }
                if (Form1.g_HomeZ == false)
                {
                    Form1.g_HomeZ = true;
                    HomeStepZ = 10;
                }
                
                
            }
            if(1 == ChoicesAxis)
            {
                if (Form1.g_HomeX == false)
                {
                    Form1.g_HomeX = true;
                    HomeStepX = 10;
                }
            }
            if (2 == ChoicesAxis)
            {
                if (Form1.g_HomeY == false)
                {
                    Form1.g_HomeY = true;
                    HomeStepY = 10;
                }
            }
            if (3 == ChoicesAxis)
            {
                if (Form1.g_HomeZ == false)
                {
                    Form1.g_HomeZ = true;
                    HomeStepZ = 10;
                }
            }
        }


        public void SingleAxisMoveAbs(int AxisNum, float AxisSpeed, float AxisPos)
        {
            int ret = 0;
            if (AxisNum == 1 || AxisNum == 2)
            {
                ret = zmcaux.ZAux_Direct_SetUnits(Form1.g_handle, AxisNum - 1, 10000);

                ret = zmcaux.ZAux_Direct_SetSpeed(Form1.g_handle, AxisNum - 1, AxisSpeed);

                ret = zmcaux.ZAux_Direct_SetAccel(Form1.g_handle, AxisNum - 1, 5 * AxisSpeed);

                ret = zmcaux.ZAux_Direct_Single_MoveAbs(Form1.g_handle, AxisNum - 1, AxisPos);
            }

            if (AxisNum == 3)
            {
                ret = zmcaux.ZAux_Direct_SetUnits(Form1.g_handle, AxisNum - 1, 1);

                ret = zmcaux.ZAux_Direct_SetSpeed(Form1.g_handle, AxisNum - 1, 10000 * AxisSpeed);

                ret = zmcaux.ZAux_Direct_SetAccel(Form1.g_handle, AxisNum - 1, 50000 * AxisSpeed);

                ret = zmcaux.ZAux_Direct_Single_MoveAbs(Form1.g_handle, AxisNum - 1, 10000 * AxisPos);
            }

 
            
       
        }

        public void stopAllAxis()
        {

            int ret = 0;
            Form1.g_HomeX = false;
            HomeStepX = 0;
            Form1.g_HomeY = false;
            HomeStepY = 0;
            Form1.g_HomeZ = false;
            HomeStepZ = 0;
            float cmd = 0;                                                                 //进入正弦运动
            zmcaux.ZAux_Direct_SetUserVar(Form1.g_handle, "gStep", cmd);          //发送运行周期数
            ret = zmcaux.ZAux_Direct_Single_Cancel(Form1.g_handle, 0, 2);
            ret = zmcaux.ZAux_Direct_Single_Cancel(Form1.g_handle, 0, 3);
            ret = zmcaux.ZAux_Direct_Single_Cancel(Form1.g_handle, 1, 2);
            ret = zmcaux.ZAux_Direct_Single_Cancel(Form1.g_handle, 1, 3);
            ret = zmcaux.ZAux_Direct_Single_Cancel(Form1.g_handle, 2, 2);
            ret = zmcaux.ZAux_Direct_Single_Cancel(Form1.g_handle, 2, 3);
            ret = zmcaux.ZAux_Direct_Rapidstop(Form1.g_handle, 2);
        }

        public void AnalyzeCMD(int Who, byte[] CMD)
        {
            int ret = 0;
            //解析指令
            //头尾校验
            if (CMD.First() != 0xeb || CMD.Last() != 0xec)
            {
                //指令格式错误
                return;
            }
            //校验和
            byte sum = 0;
            for (int i = 1; i < CMD.Length - 2; i++)
            {
                sum += CMD[i];
            }
            if (sum != CMD[CMD.Length - 2])
            {
                //指令格式错误
                return;
            }

            //解析指令
            switch (CMD[1])
            {
                case 0xFD: //获取控制权
                    if (Who == 1)
                    {
                        b_Connect_Remo = true;
                        b_Connect_Remo2 = false;
                        Console.WriteLine("通道1获得控制权");
                    }
                    if (Who == 2)
                    {
                        b_Connect_Remo2 = true;
                        b_Connect_Remo = false;
                        Console.WriteLine("通道2获得控制权");

                    }
                    return;

                    break;
                case 0xFE: //断开当前连接
                    if (Who == 1)
                    {
                        //断开连接
                        b_Connect_Remo = false;
                        Console.WriteLine("通道1断开");
                    }
                    if (Who == 2)
                    {
                        //断开连接
                        b_Connect_Remo2 = false;
                        Console.WriteLine("通道2断开");

                    }
                    return;
                    break;
                case 0xFF: //初次连接时查询当前是否有其他用户控制设备 
                    if (Who == 1)
                    {
                        if (b_Connect_Remo2)
                        {
                            b_Connect_Remo = false;
                            UDPServer.SendTo(System.Text.Encoding.ASCII.GetBytes("yes"), RemoteDevice);
                            Console.WriteLine("通道1建立连接没有控制权");
                        }
                        else
                        {
                            b_Connect_Remo = true;
                            UDPServer.SendTo(System.Text.Encoding.ASCII.GetBytes("no"), RemoteDevice);
                            Console.WriteLine("通道1建立连接获得控制权");

                        }
                    }
                    if (Who == 2)
                    {
                        if (b_Connect_Remo)
                        {
                            b_Connect_Remo2 = false;
                            UDPServer2.SendTo(System.Text.Encoding.ASCII.GetBytes("yes"), RemoteDevice2);
                            Console.WriteLine("通道2建立连接没有控制权");

                        }
                        else
                        {
                            b_Connect_Remo2 = true;
                            UDPServer2.SendTo(System.Text.Encoding.ASCII.GetBytes("no"), RemoteDevice2);
                            Console.WriteLine("通道2建立连接获得控制权");

                        }

                    }
                    return;
                    break;
                default: break;
            }



            if(Who==1)
            {
                if(b_Connect_Remo)
                {
                    Console.WriteLine("执行通道1指令");
                }
                else
                {
                    Console.WriteLine("通道1指令没有控制权");
                    return;
                }
            }
            else if(Who==2)
            {
                if (b_Connect_Remo2)
                {
                    Console.WriteLine("执行通道2指令");
                }
                else
                {
                    Console.WriteLine("通道2指令没有控制权");
                    return;
                }
            }
            else
            {
                

            }

            //解析指令
            switch (CMD[1])
            {
                case 0x01: //寻零
                    //CMD[2]表示轴号 1横移 2纵移 3滚转 0所有轴
                    //System.Windows.Forms.MessageBox.Show("寻零 "+CMD[2].ToString());
                    RemoHome((int)CMD[2]);
                    break;
                case 0x02: //设置比例（不实现）
                    break;
                case 0x03: //停止,所有轴停止
                    stopAllAxis();
                    //System.Windows.Forms.MessageBox.Show("所有轴停止");
                    break;
                case 0x04: //运动到某一位置
                    //CMD[2]表示轴号 1横移 2纵移 3滚转 0所有轴
                    //CMD[3-6]表示位置，表示毫米
                    //CMD[7-10]表示速度，表示毫米/秒
                    float TargetPosition = (CMD[3] + CMD[4] * 0x100) / 10f;
                    float Speed = 0;
                    if (CMD[2] == 1 || CMD[2] == 2)
                    {
                        Speed = (CMD[7] + CMD[8] * 0x100) / 10f;
                    }
                    else if (CMD[2] == 3)
                    {
                        Speed = (CMD[7] + CMD[8] * 0x100);
                    }

                    if (TargetPosition > 65536 / 2 / 10)
                    {
                        TargetPosition = (float)(TargetPosition - (float)6553.6);
                    }




                    SingleAxisMoveAbs(CMD[2], Speed, TargetPosition);

                    //System.Windows.Forms.MessageBox.Show("运动 " + CMD[2].ToString() + "   " + TargetPosition.ToString() + "   " + Speed.ToString());
                    break;
                case 0x05: //正弦运动
                    //CMD[2]表示轴号 1横移 2纵移 3滚转 0所有轴
                    //CMD[3-6]表示频率
                    //CMD[7-10]表示幅度,毫米
                    //CMD[11-14]表示次数
                    float pl = 1000f / BitConverter.ToInt32(CMD, 3);
                    float fd = BitConverter.ToInt32(CMD, 7) / 10f;
                    float cs = BitConverter.ToInt32(CMD, 11);
                    SinMove(CMD[2], fd, pl, cs);
                    //System.Windows.Forms.MessageBox.Show("正弦 " + CMD[2].ToString() + "   "  + pl.ToString() + "   " + fd.ToString() + " " + cs.ToString());
                    break;
                case 0x06: //设置最大加速度（不实现）
                    break;
                case 0x07: //停止,所有清零
                    stopAllAxis();
                    ret = zmcaux.ZAux_Direct_SetUnits(Form1.g_handle, 0, 10000);
                    ret = zmcaux.ZAux_Direct_SetAccel(Form1.g_handle, 0, 300);
                    ret = zmcaux.ZAux_Direct_SetDecel(Form1.g_handle, 0, 300);
                    ret = zmcaux.ZAux_Direct_SetSpeed(Form1.g_handle, 0, 50);
                    ret = zmcaux.ZAux_Direct_SetCreep(Form1.g_handle, 0, 30);
                    ret = zmcaux.ZAux_BusCmd_SetDatumOffpos(Form1.g_handle, 0, 0);
                    ret = zmcaux.ZAux_BusCmd_Datum(Form1.g_handle, 0, 35);

                    ret = zmcaux.ZAux_Direct_SetUnits(Form1.g_handle, 1, 10000);
                    ret = zmcaux.ZAux_Direct_SetAccel(Form1.g_handle, 1, 300);
                    ret = zmcaux.ZAux_Direct_SetDecel(Form1.g_handle, 1, 300);
                    ret = zmcaux.ZAux_Direct_SetSpeed(Form1.g_handle, 1, 50);
                    ret = zmcaux.ZAux_Direct_SetCreep(Form1.g_handle, 1, 30);
                    ret = zmcaux.ZAux_BusCmd_SetDatumOffpos(Form1.g_handle, 1, 0);
                    ret = zmcaux.ZAux_BusCmd_Datum(Form1.g_handle, 1, 35);

                    ret = zmcaux.ZAux_Direct_SetUnits(Form1.g_handle, 2, 10000);
                    ret = zmcaux.ZAux_Direct_SetAccel(Form1.g_handle, 2, 300);
                    ret = zmcaux.ZAux_Direct_SetDecel(Form1.g_handle, 2, 300);
                    ret = zmcaux.ZAux_Direct_SetSpeed(Form1.g_handle, 2, 50);
                    ret = zmcaux.ZAux_Direct_SetCreep(Form1.g_handle, 2, 30);
                    ret = zmcaux.ZAux_BusCmd_SetDatumOffpos(Form1.g_handle, 2, 0);
                    ret = zmcaux.ZAux_BusCmd_Datum(Form1.g_handle, 2, 35);

                    zmcaux.ZAux_Direct_SetUserVar(Form1.g_handle, "gAxisX_HomeDone", 1);          //发送幅值
                    zmcaux.ZAux_Direct_SetUserVar(Form1.g_handle, "gAxisY_HomeDone", 1);          //发送幅值
                    zmcaux.ZAux_Direct_SetUserVar(Form1.g_handle, "gAxisZ_HomeDone", 1);          //发送幅值
                    //ret = zmcaux.ZAux_BusCmd_Datum(Form1.g_handle, 0, 35);
                    //ret = zmcaux.ZAux_BusCmd_Datum(Form1.g_handle, 1, 35);
                    //ret = zmcaux.ZAux_BusCmd_Datum(Form1.g_handle, 2, 35);
                    //System.Windows.Forms.MessageBox.Show("所有轴清零");
                    break;
                //case 0x0FD: //获取控制权
                //    if (Who == 1)
                //    {
                //        b_Connect_Remo = true;
                //        b_Connect_Remo2 = false;
                //    }
                //    if (Who == 2)
                //    {
                //        b_Connect_Remo2 = true ;
                //        b_Connect_Remo = false;
                //    }
                //    break;
                //case 0x0FE: //断开当前连接
                //    if (Who == 1)
                //    {
                //        //断开连接
                //        b_Connect_Remo = false;
                //    }
                //    if (Who == 2)
                //    {
                //        //断开连接
                //        b_Connect_Remo2 = false;
                //    }
                //    break;
                //case 0x0FF: //查询当前是否有其他用户控制设备 
                //    if(Who==1)
                //    {
                //        if(b_Connect_Remo2)
                //        {
                //            UDPServer.SendTo(System.Text.Encoding.ASCII.GetBytes("yes"), RemoteDevice);
                //        }
                //        else
                //        {
                //            UDPServer.SendTo(System.Text.Encoding.ASCII.GetBytes("no"), RemoteDevice);
                //        }
                //    }
                //    if(Who==2)
                //    {
                //        if(b_Connect_Remo)
                //        {

                //            UDPServer2.SendTo(System.Text.Encoding.ASCII.GetBytes("yes"), RemoteDevice2);

                //        }
                //        else
                //        {

                //            UDPServer2.SendTo(System.Text.Encoding.ASCII.GetBytes("no"), RemoteDevice2);
                //        }

                //    }
                //    break;
                default: break;
            }
        }
    }
}
